#include <iostream>
#include <string.h>

#include <inspectrobot/serial_port.h> 

#include <unistd.h>

using namespace std;

//——————————————————————————————-serial init-串口初始化-————————————————————————————————————————————
int serial_init(serial::Serial& ser){
    try
    {
        ser.setPort("/dev/ttyUSB0");
        ser.setBaudrate(115200);

        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        //每次读取串口数据到缓存区的时间，这个时间越大，读取到缓存区数据量越大
        //括号里面的数字单位是milliseconds即毫秒

        //默认数据位为8，校验位为0，停止位为1，流控制为无
        ser.setTimeout(to);
        ser.open();//打开串口
    }
    catch(serial::IOException& e)//前面是类型，e是事件
    //若没有匹配到catch子句，此时程序会转到terminate的标准库函数，terminate会终止当前进程的执行？
    {
        ROS_ERROR_STREAM("Unable to open port.");
        return -1;
    }
    //判断串口是否打开成功
    if(ser.isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return -1;
    }
    return 0;
}

//———————————————————————————————————read——拼接函数————————————————————————————————————————————————
//read读取
void read_buffer(serial::Serial& ser)
{
  //ser为串口类实例化对象，
  
    if(ser.available()){
        ROS_INFO("读串口");
        uint8_t buffer[Frame_len]; // 
        ser.read(buffer, Frame_len);
        //ros的serial包的read()函数有重载，返回数据形式可以是unsigned char*和string两种类型
        //因为十六进制有些值不能以字符串形式显示出来，如0x00，是不可见字符
        int len = sizeof(buffer) / sizeof(buffer[0]);
    
        //数组 转 向量 赋值
        for(int i = 0; i < len; i++) {
            new_buffer[i] = buffer[i];
        }
        //打印验证
        for(int i = 0; i < new_buffer.size(); i++) {
            std::cout << std::showbase << std::hex << static_cast<int>(new_buffer[i]) << " ";
        }

    }
    else {
        ROS_INFO("暂无数据");
    }
}
//初始拼接
void put_together_init(vector<uint8_t> newread_buffer){
    cat_buffer.clear();
    cout<< "------------put_together_init--------------"<< endl;
    cout << std::dec << "cat_buffer大小" << cat_buffer.size() << endl;
    cat_buffer.insert(cat_buffer.end(),last_remaining_buffer.begin(),last_remaining_buffer.end()); //将last_remaining_buffer压入

    cat_buffer.insert(cat_buffer.end(),newread_buffer.begin(),newread_buffer.end()); //继续将new_buffer压入，压栈
    data_len = cat_buffer.size();//很重要！头文件里这么写没用。

    for(int i = 0;i < last_remaining_buffer.size(); i++){
        std::cout << std::showbase << std::hex << static_cast<int>(last_remaining_buffer[i]) << " ";                   
        //显示检查last_remaining_buffer, 输出代码没问题
    }ROS_INFO("检查last_remaining_buffer");

    for(int i = 0;i < newread_buffer.size(); i++){
        std::cout << std::showbase << std::hex << static_cast<int>(newread_buffer[i]) << " ";                   
        //显示检查newread_buffer, 输出代码没问题
    }ROS_INFO("检查newread_buffer");

    cout << std::dec << "cat_buffer大小" << cat_buffer.size() << endl;

    for(int i = 0;i < cat_buffer.size() ; i++){
        std::cout << std::showbase << std::hex << static_cast<int>(cat_buffer[i]) << " ";                   
        //显示检查cat_buffer, 输出代码没问题
    }ROS_INFO("检查cat_buffer");
}
// 如果不存在完整帧，则将剩余数据保存
void put_together_remain(int index){
    last_remaining_buffer.clear();
    if(cat_buffer.end()<= (cat_buffer.begin()+index+Frame_len)){
        last_remaining_buffer.insert(last_remaining_buffer.end(), cat_buffer.begin()+index, cat_buffer.end()); // 合并两个vector，两帧拼接
    }else{
        last_remaining_buffer.insert(last_remaining_buffer.end(), cat_buffer.begin()+index, cat_buffer.begin()+index+Frame_len);
    }

    ROS_INFO("保留last_remaining_buffer:");  
    for(int i = 0;i < last_remaining_buffer.size(); i++){      
        std::cout << std::showbase << std::hex << static_cast<int>(last_remaining_buffer[i]) << " ";
        //显示数据, 输出代码没问题
    }
    HeaderFlag = 0;
    EndFlag = 0;
    EntireFlag = 0;
}
/* //此代码在帧头有多位时启用,只保留帧头-1位数据
   // 合并两个vector，两帧拼接
   // 没找到包头，将最后帧头-1位数据字节数据保留，防止漏掉帧头
void put_together_HeaderRemain(){
    last_remaining_buffer.clear();
    last_remaining_buffer.insert(last_remaining_buffer.end(), {cat_buffer[i+1], cat_buffer[i+2]});

} */

//找帧头data_len-Header_len
void find_FrameHeader(int search_len){
    ROS_INFO("开始寻找帧头");
    sleep(1);
    int i = 0;
    for(i; i < (search_len - Header_len ); i++){//在拼接长度内寻找
        if(cat_buffer[i] ==0xfa/*此处可以用|加入多位帧头的条件判断*/ ){
            index_head = i;//帧头位置（永远是第一位下标）
            ROS_INFO("index_head为%d",index_head);
            
            if(refind_FrameHeader()== 1 && data_len >= (index_head+ Frame_len)){
                refind_FrameHeader();
            }
            ROS_INFO("已是真实帧头");
            //i = 0;
            HeaderFlag = 1;
            break;
        }    
    }
    ROS_INFO("没有找到帧头");
    
}
int refind_FrameHeader(){
    ROS_INFO("帧头再寻找");
    int i = Header_len + index_head;
    for(i ; i < (index_head + Frame_len - 1); i++){
        //从帧头后起始，在完整帧长度内再判断是否有帧头
        if(cat_buffer[i] ==0xfa ){
            index_head = i;//帧头位置
            ROS_INFO("index_head更新为%d",index_head);
            return 1;
        }

    }
    return 0;
}
//找帧尾
void find_FrameEnd(int search_len){
    int j = index_head;
    for(j; j < search_len; j++){//在拼接长度内寻找
        if(cat_buffer[j] ==0xf5 ){
            index_end = j;//标记帧尾位置，用于接下来保存
            ROS_INFO("index_end为%d",index_end);
            EndFlag = 1;
            if(cat_buffer[index_head + Frame_len - 1] ==0xf5){//数据长度刚刚好
                
                ROS_INFO("找到完整帧");                
                EntireFlag = 1;
                return;
            }
            else{
                EntireFlag = 0;
                ROS_INFO("有帧尾，长度不足或超长，再寻找");
            }
            ROS_INFO("检查break是否跳出1");    
        }
        else
            //EntireFlag = 0;
            EndFlag = 0;
            //应该从帧头开始保存拼接
    } 
    ROS_INFO("没有帧尾，再寻找");  
}
//********************************读取函数————核心函数***********************************************
int receive_msg_Decode(){
    cout<< "开始解析数据"<< endl;
    put_together_init(new_buffer);
    
    data_len = cat_buffer.size();
    find_FrameHeader(data_len);
    if(HeaderFlag == 1){
        find_FrameEnd(data_len);
        if(EndFlag == 1){
            if(EntireFlag == 1){
                ROS_INFO("解析输出/执行：接收到的命令为");
                for(int i = 0;i < Frame_len; i++){
                    
                    received_buffer[i] = cat_buffer[index_head+i];
                    //将 真实数据 提取到 数组 received_buffer，提取代码没问题
                    std::cout << std::showbase << std::hex << static_cast<int>(cat_buffer[index_head+i]) << " ";
                    
                    //显示数据, 输出代码没问题
                }
                
                put_together_remain(index_end);
                return 1; 
            }
            
            //拼接
            ROS_INFO("拼接测试");
            put_together_remain(index_end);
            return 0;
            
        }
        else{
            put_together_remain(index_head);
            return 0;
        }
    }
    else  return 0;  
        //put_together_HeaderRemain;
}
//检查使能接收请求
int check_EnableRequest(){

    int i = 0;
    while(i < Frame_len){
        if(received_buffer[i] != enable_request_buffer[i]){
            cout << "未能 接收到 使能请求"<<endl;
            return -1;
        }      
        i ++;
    }
    cout << "接收到 使能请求"<<endl;
    return 1;    
}
void RE_receive_msg_Decode(serial::Serial& ser){
    if(receive_msg_Decode()!=1){
        ROS_INFO("重接收");
        read_buffer(ser);
        //没接到完整命令就重新接收
        RE_receive_msg_Decode(ser);
    }
    else return;  
}
//机器人初始化
int robot_init(serial::Serial& ser){
    //初始化串口robot,发送使能
    //int p = serial_init(ser);
    
    read_buffer(ser);
    ROS_INFO("读了数据.");
    if(receive_msg_Decode()!=1){
        RE_receive_msg_Decode(ser);
    }
    else {
        if(check_EnableRequest()== 1){
            ROS_INFO("巡检机器人请求使能");
            ser.write(enable_buffer, 11); 
            sleep(1);
            ROS_INFO("发送巡检机器人使能命令");
            ser.write(enable_buffer, 11); 
            //再发送确保是能，这都使能不上就是单片机自己问题了
            return 1;
        } 
        else{
            robot_init(ser);
            ROS_INFO("重新初始化中");
        } 
    }   
    ROS_INFO("使能失败");
    return -1;//执行到程序尾，说明出错
}
//******************************写串口函数相关————核心函数******************************************8
int robot_move(serial::Serial& ser)
{
    //数据转 换浮点数、int 转 16进制整形
    union un{
		float fValue;
		int iValue;

        unsigned char c[sizeof(float)];
	} ;

    union un un1;
         un1.fValue= RobotYawRate_;
         move_buffer[5] = un1.c[0];
         move_buffer[4] = un1.c[1];
         move_buffer[3] = un1.c[2];
         move_buffer[2] = un1.c[3];
         
     union un un2;
         un2.iValue= RobotV_;
         move_buffer[9] = un2.c[0];
         move_buffer[8] = un2.c[1];
         move_buffer[7] = un2.c[2];
         move_buffer[6] = un2.c[3];//speed 
        //move_buffer[9] = 0x20;
    
    for(int i = 0;i < sizeof(move_buffer);i++){
        std::cout << std::showbase << std::hex << static_cast<int>(move_buffer[i]) << " ";
        //显示数据
    }
    std::cout << std::endl;
    ser.write(move_buffer, sizeof(move_buffer));

    memcpy(move_buffer,reset_buffer,sizeof(reset_buffer));
    //若发送数据Topic的节点挂掉后，由于buffer是全局变量，buffer的数据段会一直是上一帧的数据，不会再改变，为避免上述情况，在将数据写入串口设备后，将数据段清零。
    return 1; 
}
void robot_stop(serial::Serial& ser){
    ser.write(stop_buffer, sizeof(stop_buffer));
}

